#include "shared.h"
#include "webcam.h"
#include "uvc.h"





/****************************
*			UTILS			*
****************************/


static int r5u870_uvc_req( struct driverInstance* this, int cmd, u8 valhi, u8 vallow, u8 idxhi, u8 idxlow, u8* buf, int len ){
	
	int out, res, stres;
	int tries = 5;
	u8 stbuf[1];
	
	out = (cmd == UVC_SET_CUR) ? 1 : 0;
	
retry:
	//TODO: Base our other retry control message off this one.
	res = r5u870_control_msg(
		this,
		out,
		1,
		cmd,
		(valhi << 8) | vallow,
		(idxhi << 8) | idxlow,
		buf,
		len
	);
	
	if( res != -EPIPE ) goto complete;
	
	stres = r5u870_control_msg(
		this,
		0,
		1,
		UVC_GET_CUR,
		UVC_VC_REQUEST_ERROR_CODE_CONTROL << 8,
		this->vh_ctrl_ifnum,
		stbuf,
		sizeof(stbuf)
	);

	if (((stres == -EPIPE) && --tries) || ((stres == 1) && (stbuf[0] == 1) && --tries)){
		msleep(5);
		printk( KERN_ERR "[%s] uvc_req: retrying - EPIPE/stres error.\n", KBUILD_MODNAME );
		goto retry;
	}
	
	if( stres != 1 ){
		printk( KERN_ERR "[%s] uvc_req: status req failed: %d\n", KBUILD_MODNAME, stres );
		goto complete;
		
	}else printk( KERN_ERR "[%s] uvc_req: status %d\n", KBUILD_MODNAME, stbuf[0] );
	
complete:
	if( res < 0 ) printk( KERN_ERR "[%s] uvc_req %02x/%02x%02x/%02x%02x failed: %d\n", KBUILD_MODNAME, cmd, valhi, vallow, idxhi, idxlow, res );
	return res;
}


static int usbcam_claim_interface( struct driverInstance* this, int interfaceNumber ){
	
	struct usb_interface* interface;
	struct claimed_interface* interfaceCapsule;
	int res;
	
	interface = usb_ifnum_to_if( this->usbDevice, interfaceNumber );
	
	if( !interface ){
		printk( KERN_ERR "[%s] %s: interface %d does not exist\n", KBUILD_MODNAME, __FUNCTION__, interfaceNumber );
		return -ENODEV;
	}
	
	res = usb_driver_claim_interface( this->usbDeviceDriver, interface, NULL );
	if( res ) return res;
	
	interfaceCapsule = devm_kzalloc( this->dev, sizeof( struct claimed_interface ), GFP_KERNEL );
	if( !interfaceCapsule ){
		usb_driver_release_interface( this->usbDeviceDriver, interface );
		return -ENOMEM;
	}
	
	INIT_LIST_HEAD( &interfaceCapsule->listItem );// ???
	interfaceCapsule->interface = usb_get_intf( interface );
	usb_set_intfdata( interface, this );
	list_add_tail( &interfaceCapsule->listItem, &this->claimedInterfacesList );
	
	return res;
}

/****************************
*	PACKET PROCESSING		*
****************************/


int r5u870_decide_pkt_uvc( struct driverInstance* this, int pktstatus, int pktlen, const u8* pktdata, int* start ){
	
	if( !pktlen ){
		this->vh_emptypkts++;
		if ( this->vh_emptypkts >= R5U870_EMPTYPKT_GIVE_UP ){
			if( printk_ratelimit() ) printk( KERN_ERR "[%s] capture abort: too many empty pkts\n", KBUILD_MODNAME );
			return -EIO;
		}
		if ( this->vh_frame_accum < 0 ) return -EPIPE;
		return -EAGAIN;
	}
	
	this->vh_emptypkts = 0;
	if( this->vh_frame_accum < 0 ){
		if( pktdata[1] & 2 ) this->vh_frame_accum = 0;
		return -EPIPE;
	}
	
	if( ( pktdata[0] < 2 ) || ( pktdata[0] > pktlen ) ){
		if( printk_ratelimit() ) printk( KERN_ERR "[%s] capture abort: hdrlen=%d pktlen=%d\n", KBUILD_MODNAME, pktdata[0], pktlen );
		return -EIO;
	}
	this->vh_frame_accum += pktlen - pktdata[0];
	if( this->vh_frame_accum > this->webcamCurrentPixelformat.sizeimage ){
		if( printk_ratelimit() ) printk( KERN_ERR "[%s] frame abort: accum=%d\n", KBUILD_MODNAME, this->vh_frame_accum );
		this->vh_frame_accum = -1;
		return -EPIPE;
	}
	
	*start = pktdata[0];
	if( pktdata[1] & 2 ){
		if( this->vh_frame_accum < this->webcamCurrentPixelformat.sizeimage ){
			if( printk_ratelimit() ) printk( KERN_WARNING "[%s] short frame (exp:%d got:%d)\n", KBUILD_MODNAME, this->webcamCurrentPixelformat.sizeimage, this->vh_frame_accum );
		}
		this->vh_frame_accum = 0;
		return 0;
	}
	return -EAGAIN;
}



//UVC capture is controlled by changing the altsetting
int r5u870_cap_stop_uvc( struct driverInstance* this ){
	return 0;
}


/****************************
*			FORMATS			*
****************************/

//r5u870_set_fmt_uvc
int webcamApplyFormat_uvc( struct driverInstance* this ){
	
	unsigned char buf[26];
	int res;
	
	memset( buf, 0, sizeof(buf) );
	
	buf[2] = this->webcamCurrentFormat->formatId;
	buf[3] = this->webcamCurrentResolution->resolutionId;
	*(__le32 *) &buf[4] = cpu_to_le32( this->webcamCurrentResolution->rw_interval );
	
	printk( KERN_NOTICE "[%s] set_format: fmtidx:%d frameidx:%d %dx%d ival:%d\n",
	KBUILD_MODNAME,
	this->webcamCurrentFormat->formatId,
	this->webcamCurrentResolution->resolutionId,
	this->webcamCurrentResolution->width,
	this->webcamCurrentResolution->height,
	this->webcamCurrentResolution->rw_interval );
		
	res = r5u870_uvc_req( this, UVC_SET_CUR, UVC_VS_PROBE_CONTROL, 0, 0, this->urbstreamInterfaceNumber, buf, sizeof(buf) );
	if( res != sizeof(buf) ){
		printk( KERN_ERR "[%s] probe_control set_cur1: short write %d\n", KBUILD_MODNAME, res );
		return -EIO;
	}
	
	res = r5u870_uvc_req( this, UVC_GET_CUR, UVC_VS_PROBE_CONTROL, 0, 0, this->urbstreamInterfaceNumber, buf, sizeof(buf) );
	if ( res != sizeof(buf) ){
		printk( KERN_ERR "[%s] probe_control get_cur: short read %d\n", KBUILD_MODNAME, res );
		return -EIO;
	}
	
	if( buf[2] != this->webcamCurrentFormat->formatId ){
		printk( KERN_ERR "[%s] probe_control get_cur: got fmt %d\n", KBUILD_MODNAME, buf[2] );
		return -EIO;
	}
	
	if( buf[3] != this->webcamCurrentResolution->resolutionId ){
		printk( KERN_ERR "[%s] probe_control get_cur: got frame idx %d\n", KBUILD_MODNAME, buf[3] );
		return -EIO;
	}
	
	res = r5u870_uvc_req( this, UVC_SET_CUR, UVC_VS_COMMIT_CONTROL, 0, 0, this->urbstreamInterfaceNumber, buf, sizeof(buf) );
	if ( res != sizeof(buf) ){
		printk( KERN_ERR "[%s] commit_control set_cur2: short write %d\n", KBUILD_MODNAME, res );
		return -EIO;
	}
	
	this->urbstreamMinPacketSize = le32_to_cpup((__le32 *) &buf[22]);
	
	return 0;
}


static int r5u870_uvc_add_resolution( struct driverInstance* this, struct r5u870_pixelformat* fmtp, int width, int height, int reqbw, int frameidx, int interval ){
	
	int i;
	struct r5u870_resolution* resp;
	
	if( !width || !height ){
		printk( KERN_ERR "[%s] invalid resolution descriptor %d at %dx%d\n", KBUILD_MODNAME, frameidx, width, height );
		return -EINVAL;
	}

	resp = NULL;
	for( i = 0; i < fmtp->resolutionTableAlloc; i++ ){
		if( !fmtp->resolutionTable[i].width ){
			resp = (struct r5u870_resolution*) &fmtp->resolutionTable[i];
			break;
		}
	}
	
	if( !resp ){
		printk( KERN_NOTICE "[%s] no space for resolution descriptor %d at %dx%d\n", KBUILD_MODNAME, frameidx, width, height );
		return 0;
	}
	
	resp->width = width;
	resp->height = height;
	resp->resolutionId = frameidx;
	resp->requiredBandwidth = reqbw;
	resp->rw_interval = interval;
	
	printk( KERN_NOTICE "[%s] Found resolution %d: %dx%d ival %d (%d B/s)\n", KBUILD_MODNAME, frameidx, width, height, interval, reqbw );
	
	return 0;
}

static int r5u870_uvc_add_fmt( struct driverInstance* this, const u8* guid, int fmtidx, int nresolutions, struct r5u870_pixelformat** new_fmt ){
	
	const struct r5u870_uvc_fmtinfo* formatTemplate = NULL;
	struct r5u870_pixelformat* newFormats;
	struct r5u870_pixelformat* newFormatPointer;
	int i;
	
	for( i = 0; r5u870_uvc_fmts[i].fi_name != NULL; i++ ){
		if( !memcmp(r5u870_uvc_fmts[i].fi_guid, guid, 16) ){
			formatTemplate = &r5u870_uvc_fmts[i];
			break;
		}
	}
	
	if( formatTemplate == NULL ){
		printk( KERN_NOTICE "[%s] unknown format "
			"%02x%02x%02x%02x-%02x%02x-%02x%02x-%02x%02x-"
			"%02x%02x%02x%02x%02x%02x\n", KBUILD_MODNAME, 
			guid[0],  guid[1],  guid[2],  guid[3],
			guid[4],  guid[5],  guid[6],  guid[7],
			guid[8],  guid[9],  guid[10], guid[11], 
			guid[12], guid[13], guid[14], guid[15] 
		);
		return -ENOENT;
	}
	
	newFormats = devm_kzalloc( this->dev, (this->webcamFormatsCount+1) * sizeof( struct r5u870_pixelformat ), GFP_KERNEL );
	if( !newFormats )return -ENOMEM;
	
	if( this->webcamFormatsCount ) memcpy( newFormats, this->webcamFormats, this->webcamFormatsCount * sizeof(struct r5u870_pixelformat) );
	
	newFormatPointer = &newFormats[this->webcamFormatsCount];
	strlcpy( newFormatPointer->description, formatTemplate->fi_name, sizeof(newFormatPointer->description) );
	newFormatPointer->pixelformat = formatTemplate->fi_v4l_id;
	newFormatPointer->formatId = fmtidx;
	
	newFormatPointer->resolutionTable = devm_kzalloc( this->dev, (nresolutions+1) * sizeof( struct r5u870_resolution ), GFP_KERNEL );
	if( !newFormatPointer->resolutionTable ) return -ENOMEM;
	newFormatPointer->resolutionTableAlloc = nresolutions;
	
	/*
	Memory leak?
	If there is more than one format, then this function will be called 
	more than once. In this case, the variable this->webcamFormats contains 
	a pointer to memory, which was allocated the previous time. If this
	memory is not freed now, the pointer to it will be lost. I don't have a
	UVC camera to test it. Perhaps this is not a mistake and was conceived
	so and I did not fully understand the idea. Anyway, I use devm_kzalloc to
	allocate memory, so all memory is freed anyway when the driver is unloaded.
	*/
	//void devm_kfree( struct device* dev, void* p );
	this->webcamFormats = newFormats;
	this->webcamFormatsCount++;
	
	if( new_fmt ) *new_fmt = newFormatPointer;
	
	printk( KERN_NOTICE "[%s] Found format %d: %c%c%c%c (%d frames)\n",
		KBUILD_MODNAME,
		fmtidx,
		newFormatPointer->pixelformat & 0xff,
		( newFormatPointer->pixelformat >> 8) & 0xff,
		( newFormatPointer->pixelformat >> 16) & 0xff,
		( newFormatPointer->pixelformat >> 24) & 0xff,
		nresolutions
	);
	
	return 0;
}

/****************************
*		CONTROLS			*
****************************/

static int r5u870_uvc_ctrl_req( struct driverInstance* this, const struct webcam_control* ctrlp, int req, int* value ){
	
	u8 buf[4];
	int size, i, val, res;
	
	size = ctrlp->size;
	if( req == UVC_GET_INFO ) size = 1;
	
	if( size > sizeof(buf) ){
		printk( KERN_ERR "[%s] Control ID %d is too large, %d bytes\n", KBUILD_MODNAME, ctrlp->reg, size );
		return -EINVAL;
	}
	
	memset( buf, 0, sizeof(buf) );
	
	if( req != UVC_SET_CUR ){
		res = r5u870_uvc_req( this, req, ctrlp->reg, 0, ctrlp->unit, this->vh_ctrl_ifnum, buf, size );
		if( res < 0 ) return res;
		if( res != size ){
			printk( KERN_ERR "[%s] short read for UVC control %d\n", KBUILD_MODNAME, ctrlp->reg );
			return -EIO;
		}
		
		val = 0;
		switch( size ){
			
			case 1:
				val = buf[0];
				break;
			case 2:
				val = le16_to_cpu(*(__le16 *) buf);
				break;
			case 4:
				val = le32_to_cpu(*(__le32 *) buf);
				break;
			default:
				return -EINVAL;
		}
		
		*value = val;
		return 0;
	}
	
	val = *value;
	for( i = 0; i < size; i++, val >>= 8 ) buf[i] = val & 0xff;
	
	res = r5u870_uvc_req( this, UVC_SET_CUR, ctrlp->reg, 0, ctrlp->unit, this->vh_ctrl_ifnum, buf, size );
	if( res < 0 ) return res;
	if( res != size ){
		printk( KERN_ERR "[%s] short write for UVC control %d\n", KBUILD_MODNAME, ctrlp->reg );
		return -EIO;
	}
	return 0;
}

//r5u870_set_ctrl_uvc replaced with
static int webcamSetControlCallback_uvc( struct v4l2_ctrl* control ){
	
	struct driverInstance* this = container_of( control->handler, struct driverInstance, webcamControlHandler );
	struct webcam_control* webcamControl;
	int val;
	int res = 0;
	
	if( !control->priv ){
		printk( KERN_ERR "[%s] %s: control->priv == NULL\n", KBUILD_MODNAME, __FUNCTION__ );
		return -EINVAL;
	}
	
	webcamControl = control->priv;
	
	if( !(webcamControl->info & 2) ){
		if( printk_ratelimit() ) printk( KERN_WARNING "[%s] control %s <= %d [set not supported]\n", KBUILD_MODNAME, webcamControl->control.name, control->val );
		return -EINVAL;
	}
	
	val = control->val;
	res = r5u870_uvc_ctrl_req( this, webcamControl, UVC_SET_CUR, &val );
	if( res ) printk( KERN_ERR "[%s] FAILED set %s/uvc %02x <= %d\n", KBUILD_MODNAME, webcamControl->control.name, webcamControl->reg, control->val );
	return res;
}


//Initialize a control structure based on a UVC descriptor array,
//and a bit within a supported control bitmap.
static int r5u870_uvc_init_ctrl( struct driverInstance* this, int unit, int bit_offset ){
	
	const struct r5u870_uvc_ctrlinfo* cip = NULL;
	struct webcam_control* ctrlp;
	int i, res, val;
	
	for( i = 0; r5u870_uvc_proc_ctrls[i].ci_name != NULL; i++ ){
		if( r5u870_uvc_proc_ctrls[i].ci_bm_index == bit_offset ){
			cip = &r5u870_uvc_proc_ctrls[i];
			break;
		}
	}
	
	if( !cip ) return -ENOENT;
	
	ctrlp = devm_kzalloc( this->dev, sizeof( struct webcam_control ), GFP_KERNEL );
	if( !ctrlp ) return -ENOMEM;
	
	ctrlp->control.ops = &webcamControlOps_uvc;
	ctrlp->control.id = cip->ci_v4l_id;
	ctrlp->control.type = cip->ci_v4l_type;
	ctrlp->control.name = cip->ci_name;
	ctrlp->control.flags = cip->ci_v4l_flags;
	if( ctrlp->control.type != V4L2_CTRL_TYPE_MENU ) ctrlp->control.step = 1;
	ctrlp->control.qmenu = cip->ci_menu_names;
	
	ctrlp->reg = cip->ci_reg;
	ctrlp->unit = unit;
	ctrlp->size = cip->ci_size;
	
	res = r5u870_uvc_ctrl_req( this, ctrlp, UVC_GET_INFO, &val );
	if( res ) return res;
	ctrlp->info = val;
	
	if( cip->ci_min_force ) ctrlp->control.min = cip->ci_min;
	else{
		res = r5u870_uvc_ctrl_req( this, ctrlp, UVC_GET_MIN, &val );
		if( res < 0 ) return res;
		ctrlp->control.min = val;
	}
	
	if( cip->ci_max_force ) ctrlp->control.max = cip->ci_max;
	else{
		res = r5u870_uvc_ctrl_req( this, ctrlp, UVC_GET_MAX, &val );
		if ( res < 0 ) return res;
		ctrlp->control.max = val;
	}
	
	if( cip->ci_def_force ) ctrlp->control.def = cip->ci_def;
	else{
		res = r5u870_uvc_ctrl_req( this, ctrlp, UVC_GET_DEF, &val );
		if( res ) return res;
		ctrlp->control.def = val;
	}
	
	v4l2_ctrl_new_custom( &this->webcamControlHandler, &ctrlp->control, ctrlp );
	if( this->webcamControlHandler.error ){
		res = this->webcamControlHandler.error;
		v4l2_ctrl_handler_free( &this->webcamControlHandler );
		printk( KERN_ERR "[%s] add uvc custom control failed\n", KBUILD_MODNAME );
		return res;
	}
	
	printk( KERN_NOTICE "[%s] Found UVC control %s [%lld,%lld] def:%lld info:%02x\n",
		KBUILD_MODNAME,
		ctrlp->control.name,
		ctrlp->control.min,
		ctrlp->control.max,
		ctrlp->control.def,
		ctrlp->info
	);
	
	return 0;
}

static int r5u870_uvc_add_ctrls( struct driverInstance* this, int unit, const u8* bits, int nbits ){
	
	int i, res;
	
	//TODO: Find a beautiful way to calculate the number of controls
	v4l2_ctrl_handler_init( &this->webcamControlHandler, 8 );
	
	for( i = 0; i < nbits; i++ ){
		if( !test_bit(i, (const unsigned long *) bits) ) continue;
		
		res = r5u870_uvc_init_ctrl( this, unit, i );
		
		if( res == -ENOENT ) printk( KERN_NOTICE "[%s] unit %d ctrl %d not recognized\n", KBUILD_MODNAME, unit, i );
		else if( res ) return res;
	}
	return 0;
}


/****************************
*		UVC PARSE			*
****************************/

static int r5u870_uvc_parse_vs( struct driverInstance* this, int ifnum ){
	
	struct usb_interface* intf;
	struct usb_host_interface* aintf;
	struct r5u870_pixelformat* curfmt = NULL;
	u8* desc;
	int dlen, rlen;
	int wid, hgt, reqbw, ival;
	int res;
	
	intf = usb_ifnum_to_if( this->usbDevice, ifnum );
	if( !intf ) return -EINVAL;
	
	aintf = intf->cur_altsetting;
	
	for( desc = aintf->extra, rlen = aintf->extralen; rlen > 2; rlen -= desc[0], desc += desc[0] ){
		
		dlen = desc[0];
		if( dlen < 2) return -EINVAL;
		if( desc[1] != USB_DT_CS_INTERFACE ) continue;
		if( dlen < 3 ) return -EINVAL;
		
		switch( desc[2] ){
			
			case UVC_VS_INPUT_HEADER:
				if( dlen < 7 ){
					printk( KERN_ERR "[%s] VS_INPUT_HEADER too short at %d bytes\n", KBUILD_MODNAME, dlen );
					return -EINVAL;
				}
				this->urbstreamEndpoint = desc[6] & 0xf;
				break;
				
			case UVC_VS_FORMAT_UNCOMPRESSED:
				if( dlen < 21 ) {
					printk( KERN_ERR "[%s] VS_FORMAT_UNCOMP too short at %d bytes\n", KBUILD_MODNAME, dlen);
					break;
				}
				res = r5u870_uvc_add_fmt( this, &desc[5], desc[3], desc[4], &curfmt );
				if( res ){
					if( res != -ENOENT ) return res;
					curfmt = NULL;
				}
				break;
				
			case UVC_VS_FRAME_UNCOMPRESSED:
				if( dlen < 26 ){
					printk( KERN_ERR "[%s] VS_FRAME_UNCOMP too short at %d bytes\n", KBUILD_MODNAME, dlen );
					break;
				}
				if( !curfmt ){
					printk( KERN_NOTICE "[%s] VS_FRAME_UNCOMP not following VS_FORMAT_UNCOMP\n", KBUILD_MODNAME );
					break;
				}
				
				wid = desc[5] | (desc[6]<<8);
				hgt = desc[7] | (desc[8]<<8);
				reqbw = desc[13] | (desc[14]<<8) | (desc[15]<<16) | (desc[16]<<24);
				reqbw = (reqbw + 7) / 8;
				ival = le32_to_cpup((__le32 *) &desc[21]);
				
				res = r5u870_uvc_add_resolution( this, curfmt, wid, hgt, reqbw, desc[3], ival );
				if( res ) return res;
				break;
		}
	}
	
	return 0;
}


int r5u870_uvc_parse_vc( struct driverInstance* this ){
	
	struct usb_host_interface *aintf = this->usbInterface->cur_altsetting;
	u8 *desc;
	int dlen, rlen, count;
	int i, res;
	
	this->vh_ctrl_ifnum = aintf->desc.bInterfaceNumber;
	
	for( desc = aintf->extra, rlen = aintf->extralen; rlen > 2; rlen -= desc[0], desc += desc[0] ){
		
		dlen = desc[0];
		if ( dlen < 2 ) return -EINVAL;
		if ( desc[1] != USB_DT_CS_INTERFACE ) continue;
		if ( dlen < 3 ) return -EINVAL;

		switch( desc[2] ){
			
			case UVC_VC_HEADER:
				count = (dlen < 12) ? 0 : desc[11];
				if( dlen < (12 + count) ){
					printk( KERN_ERR "[%s] VC_HEADER too short at %d bytes\n", KBUILD_MODNAME, dlen );
					return -EINVAL;
				}
				
				for( i = 0; i < count; i++ ){
					res = usbcam_claim_interface( this, desc[12 + i] );
					if( res ) printk( KERN_ERR "[%s] interface %d already claimed\n", KBUILD_MODNAME, desc[12 + i] );
					this->urbstreamInterfaceNumber = desc[12 + i];
					
					res = r5u870_uvc_parse_vs( this, desc[12 + i] );
					if( res ) return res;
				}
				break;
				
			case UVC_VC_PROCESSING_UNIT:
				count = ( dlen < 8 ) ? 0 : desc[7];
				if( dlen < (8 + count) ){
					printk( KERN_ERR "[%s] VC_PROCESSING_UNIT too short at %d bytes\n", KBUILD_MODNAME, dlen );
					return -EINVAL;
				}
				
				res = r5u870_uvc_add_ctrls( this, desc[3], &desc[8], desc[7] * 8 );
				if( res ) return res;
				break;
		}
	}
	
	return 0;
}









































